using System;
using BepuUtilities;
using DemoContentLoader;
using DemoRenderer;
using BepuPhysics;
using BepuPhysics.Collidables;
using BepuPhysics.CollisionDetection.CollisionTasks;
using System.Diagnostics;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace Demos.SpecializedTests
{
    public class CylinderTestDemo : Demo
    {
        private static void BruteForceSearch(in Vector3 lineOrigin, in Vector3 lineDirection, float halfLength, in Cylinder cylinder, out float closestT, out float closestDistanceSquared, out float errorMargin)
        {
            const int sampleCount = 1 << 20;
            var inverseSampleCount = 1.0 / (sampleCount - 1);
            errorMargin = (float)inverseSampleCount;
            var radiusSquared = cylinder.Radius * cylinder.Radius;
            closestDistanceSquared = float.MaxValue;
            closestT = float.MaxValue;
            for (int i = 0; i < sampleCount; ++i)
            {
                var t = (float)(halfLength * (i * inverseSampleCount * 2 - 1));
                var point = lineOrigin + lineDirection * t;
                var horizontalLengthSquared = point.X * point.X + point.Z * point.Z;
                Vector3 clamped;
                if (horizontalLengthSquared > radiusSquared)
                {
                    var scale = cylinder.Radius / MathF.Sqrt(horizontalLengthSquared);
                    clamped.X = scale * point.X;
                    clamped.Z = scale * point.Z;
                }
                else
                {
                    clamped.X = point.X;
                    clamped.Z = point.Z;
                }
                clamped.Y = MathF.Max(-cylinder.HalfLength, MathF.Min(cylinder.HalfLength, point.Y));
                var distanceSquared = Vector3.DistanceSquared(clamped, point);
                if (distanceSquared < closestDistanceSquared)
                {
                    closestDistanceSquared = distanceSquared;
                    closestT = t;
                }
            }

        }

        private static void TestSegmentCylinder()
        {
            var cylinder = new Cylinder(0.5f, 1);
            CylinderWide cylinderWide = default;
            cylinderWide.Broadcast(cylinder);
            Random random = new Random(5);
            // 双精度totalIntervalError=0;
            // Double SumOfSquaredIntervalError=0;

            double totalBruteError = 0;
            double sumOfSquaredBruteError = 0;

            double totalBruteDistanceError = 0;
            double sumOfSquaredBruteDistanceError = 0;

            // 长迭代Sum=0;
            // 长迭代平方和=0;
            var capsuleTests = 1000;

            int warmupCount = 32;
            int innerIterations = 128;
            long testTicks = 0;
            for (int i = 0; i < warmupCount + capsuleTests; ++i)
            {
                Vector3 randomPointNearCylinder;
                var capsule = new Capsule(0.2f + .8f * (float)random.NextDouble(), 0.2f + 0.8f * (float)random.NextDouble());
                var minimumDistance = 1f * (cylinder.Radius + cylinder.HalfLength);
                var minimumDistanceSquared = minimumDistance * minimumDistance;
                while (true)
                {
                    randomPointNearCylinder = new Vector3((cylinder.Radius + capsule.HalfLength) * 2, (cylinder.HalfLength + capsule.HalfLength) * 2, (cylinder.Radius + capsule.HalfLength) * 2) *
                        (new Vector3(2) * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) - Vector3.One);
                    var pointOnCylinderAxis = new Vector3(0, MathF.Max(-cylinder.HalfLength, MathF.Min(cylinder.HalfLength, randomPointNearCylinder.Y)), 0);
                    var offset = randomPointNearCylinder - pointOnCylinderAxis;
                    var lengthSquared = offset.LengthSquared();
                    if (lengthSquared > minimumDistanceSquared)
                        break;
                }

                Vector3 direction;
                float directionLengthSquared;
                do
                {
                    direction = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * new Vector3(2) - Vector3.One;
                    directionLengthSquared = direction.LengthSquared();
                } while (directionLengthSquared < 1e-8f);
                direction /= MathF.Sqrt(directionLengthSquared);


                Vector3Wide.Broadcast(randomPointNearCylinder, out var capsuleOrigin);
                Vector3Wide.Broadcast(direction, out var capsuleY);

                // CapsuleCylinderTester.GetClosestPointBetweenLineSegmentAndCylinder(capsuleOrigin,封装Y,新矢量(封装半长度),柱面宽,输出变量t,输出变量最小值,输出变量最大值,输出变量偏移量从柱面到线段,输出变量迭代必填);

                // CapsuleCylinderTester.GetClosestPointBetweenLineSegmentAndCylinder(capsuleOrigin,封装Y,新矢量<浮动>(封装半长度),柱面宽度,输出变量t,输出变量偏移从柱面到线段);
                Vector<float> t = default;
                Vector3Wide offsetFromCylinderToLineSegment = default;
                var innerStart = Stopwatch.GetTimestamp();
                for (int j = 0; j < innerIterations; ++j)
                {
                    CapsuleCylinderTester.GetClosestPointBetweenLineSegmentAndCylinder(capsuleOrigin, capsuleY, new Vector<float>(capsule.HalfLength), cylinderWide, Vector<int>.Zero, out t, out offsetFromCylinderToLineSegment);
                }
                var innerStop = Stopwatch.GetTimestamp();
                if (i > warmupCount)
                {
                    testTicks += innerStop - innerStart;
                }
                Vector3Wide.LengthSquared(offsetFromCylinderToLineSegment, out var distanceSquaredWide);
                var distanceSquared = distanceSquaredWide[0];

                // IterationsSum+=IterationsRequired[0];
                // iterationsSquaredSum+=iterationsRequired[0]*iterationsRequired[0];

                BruteForceSearch(randomPointNearCylinder, direction, capsule.HalfLength, cylinder, out var bruteT, out var bruteDistanceSquared, out var errorMargin);
                var errorRelativeToBrute = MathF.Max(MathF.Abs(bruteT - t[0]), errorMargin) - errorMargin;
                sumOfSquaredBruteError += errorRelativeToBrute * errorRelativeToBrute;
                totalBruteError += errorRelativeToBrute;

                if ((distanceSquared == 0) != (bruteDistanceSquared == 0))
                {
                    Console.WriteLine($"Search and brute force disagree on intersecting distance; search found {distanceSquared}, brute found {bruteDistanceSquared}");
                }

                var bruteDistanceError = MathF.Abs(MathF.Sqrt(distanceSquared) - MathF.Sqrt(bruteDistanceSquared));
                sumOfSquaredBruteDistanceError += bruteDistanceError * bruteDistanceError;
                totalBruteDistanceError += bruteDistanceError;

                // var intervalSpan=Vector.Abs(max-min)[0];
                // sumOfSquaredIntervalError+=intervalSpan*intervalSpan;
                // totalIntervalError+=间隔跨度;


            }
            Console.WriteLine($"Average time per test (ns): {1e9 * testTicks / (innerIterations * capsuleTests * Stopwatch.Frequency)}");

            // var ververageIntervalSpan=totalIntervalError/封装测试;
            // var ververageIntervalSquaredSpan=sumOfSquaredIntervalError/封装测试;
            // var intervalStandardDeation=Math.Sqrt(Math.Max(0,verageIntervalSquaredSpan-verageIntervalSpan*verageIntervalSpan));
            // Console.WriteLine($"平均间隔跨度：{verageIntervalSpan},stddev{intervalStandardDeumation}");

            var averageBruteError = totalBruteError / capsuleTests;
            var averageBruteSquaredError = sumOfSquaredBruteError / capsuleTests;
            var bruteStandardDeviation = Math.Sqrt(Math.Max(0, averageBruteSquaredError - averageBruteError * averageBruteError));
            Console.WriteLine($"Average brute T error: {averageBruteError}, stddev {bruteStandardDeviation}");

            var averageBruteDistanceError = totalBruteDistanceError / capsuleTests;
            var averageBruteDistanceSquaredError = sumOfSquaredBruteDistanceError / capsuleTests;
            var bruteDistanceStandardDeviation = Math.Sqrt(Math.Max(0, averageBruteSquaredError - averageBruteError * averageBruteError));
            Console.WriteLine($"Average brute distance error: {averageBruteDistanceError}, stddev {bruteDistanceStandardDeviation}");

            // VAR AverageIterations=(双倍)迭代求和/封装测试;
            // var AverageIterationSquared=(双倍)迭代平方和/封装测试;
            // var iterationStandardDeation=Math.Sqrt(Math.Max(0,averageIterationSquared-AverageIterations*AverageIterations));
            // Console.WriteLine($"平均迭代次数：{averageIterations},stddev{iterationStandardDeumation}");
        }

        public override void Initialize(ContentArchive content, Camera camera)
        {
            camera.Position = new Vector3(10, 0, 6);
            camera.Pitch = 0;
            camera.Yaw = 0;

            Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0f, 0)), new PositionFirstTimestepper());

            var cylinderShape = new Cylinder(1f, .2f);
            cylinderShape.ComputeInertia(1, out var cylinderInertia);
            var cylinder = BodyDescription.CreateDynamic(new Vector3(10f, 3, 0), cylinderInertia, new CollidableDescription(Simulation.Shapes.Add(cylinderShape), 1000), new BodyActivityDescription(0.01f));
            Simulation.Bodies.Add(cylinder);
            Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new RigidPose(new Vector3(0, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), MathHelper.PiOver4)), Simulation.Shapes, new Sphere(2)));
            Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new RigidPose(new Vector3(7, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), MathHelper.PiOver4)), Simulation.Shapes, new Capsule(0.5f, 1f)));
            Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new RigidPose(new Vector3(21, -3, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0)), Simulation.Shapes, new Box(3f, 1f, 3f)));
            Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new RigidPose(new Vector3(28, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0)), Simulation.Shapes,
                new Triangle(new Vector3(10f, 0, 10f), new Vector3(14f, 0, 10f), new Vector3(10f, 0, 14f))));
            Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new RigidPose(new Vector3(14, -6, 0), QuaternionEx.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 0, 1)), 0)), Simulation.Shapes, new Cylinder(3f, .2f)));


            cylinderShape = new Cylinder(1f, 3);
            var cylinderShapeIndex = Simulation.Shapes.Add(cylinderShape);
            cylinderShape.ComputeInertia(1, out cylinderInertia);
            // const int rowCount=15;
            // for(int rowIndex=0;rowIndex<rowCount;++rowIndex)
            // {
            // int column nCount=rowCount-rowIndex;
            // for(int column nIndex=0;column nIndex<column nCount;++column nIndex)
            // {
            // Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(
            // (-column Count*0.5f+column nIndex)*CylinderShape.Radius*2f,
            // (rowIndex+0.5F)*CylinderShape.Length-9.5F,-10),
            // Inertia圆柱体,
            // 新的CollidableDescription(CylinderShapeIndex,0.1F),
            // New BodyActivityDescription(0.01f));
            // }
            // }

            var box = new Box(1f, 3f, 2f);
            var capsule = new Capsule(1f, 1f);
            var sphere = new Sphere(1.5f);
            box.ComputeInertia(1, out var boxInertia);
            capsule.ComputeInertia(1, out var capsuleInertia);
            sphere.ComputeInertia(1, out var sphereInertia);
            var boxIndex = Simulation.Shapes.Add(box);
            var capsuleIndex = Simulation.Shapes.Add(capsule);
            var sphereIndex = Simulation.Shapes.Add(sphere);
            const int width = 2;
            const int height = 1;
            const int length = 2;
            for (int i = 0; i < width; ++i)
            {
                for (int j = 0; j < height; ++j)
                {
                    for (int k = 0; k < length; ++k)
                    {
                        var location = new Vector3(5, 3, 5) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 2.5f, -30 - length * 1.5f);
                        var bodyDescription = new BodyDescription
                        {
                            Activity = new BodyActivityDescription(-0.01f),
                            Pose = new RigidPose
                            {
                                Orientation = Quaternion.Identity,// Quaternion.CreateFromAxisAngle(Vector3.Normalize(new向量3(1,1,1)),MathF.PI*0.79813f),
                                Position = location
                            },
                            Collidable = new CollidableDescription
                            {
                                Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete },
                                SpeculativeMargin = 0.1f
                            }
                        };
                        switch (j % 4)
                        {
                            case 0:
                            // bodyDescription.Collidable.Shape=boxIndex;
                            // bodyDescription.LocalInertia=boxInertia;
                            // 断线;
                            case 1:
                            // bodyDescription.Collidable.Shape=胶囊索引;
                            // bodyDescription.LocalInertia=胶囊Inertia;
                            // 断线;
                            case 2:
                            // bodyDescription.Collidable.Shape=spherIndex;
                            // bodyDescription.LocalInertia=sphere Inertia;
                            // 断线;
                            case 3:
                                bodyDescription.Collidable.Shape = cylinderShapeIndex;
                                bodyDescription.LocalInertia = cylinderInertia;
                                break;
                        }
                        Simulation.Bodies.Add(bodyDescription);

                    }
                }
            }

            const int planeWidth = 50;
            const int planeHeight = 50;
            DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight,
                (int x, int y) =>
                {
                    var octave0 = (MathF.Sin((x + 5f) * 0.05f) + MathF.Sin((y + 11) * 0.05f)) * 3f;
                    var octave1 = (MathF.Sin((x + 17) * 0.15f) + MathF.Sin((y + 19) * 0.15f)) * 2f;
                    var octave2 = (MathF.Sin((x + 37) * 0.35f) + MathF.Sin((y + 93) * 0.35f)) * 1f;
                    var octave3 = (MathF.Sin((x + 53) * 0.65f) + MathF.Sin((y + 47) * 0.65f)) * 0.5f;
                    var octave4 = (MathF.Sin((x + 67) * 1.50f) + MathF.Sin((y + 13) * 1.5f)) * 0.25f;
                    return new Vector3(x, octave0 + octave1 + octave2 + octave3 + octave4, y);
                }, new Vector3(4, 1, 4), BufferPool, out var planeMesh);
            Simulation.Statics.Add(new StaticDescription(new Vector3(-100, -15, 100), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2),
                new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f)));

            // Simulation.Statics.Add(new StaticDescription(new Vector3(0,-10,0),Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1,0,1)),0),Simulation.Shapes.Add(New Cylinder(100,1f),0.1F);

            // {
            // CapsuleCylinderTester测试仪=默认值;
            // CapsuleWide a=默认值;
            // a.广播(新胶囊(0.5F,1));
            // 柱面宽度b=默认值;
            // b.广播(新气缸(0.5F,1));
            // var speculativeMargin=new Vector<Float>(2f);
            // Vector3Wide.Broadcast(new Vector3(0,-0.4F,0),out var offsetB);
            // QuaternionWide.Broadcast(Quaternion.CreateFromAxisAngle(new向量3(1,0,0),MathHelper.PiOver2),外部变量方向A);
            // QuaternionWide.Broadcast(Quaternion.CreateFromAxisAngle(new矢量3(1,0,0),0),外部变量方向B);
            // tester.test(ref a,ref b,ref speculativeMargin,ref offsetB,ref OrientationA,ref OrientationB,Vector<Float>.Count,out var流形);
            // }
            // {
            // 柱面宽度a=默认值;
            // a.广播(新圆筒(0.5F,1F));
            // 柱面宽度b=默认值;
            // b.广播(新气缸(0.5f,1f));
            // var supportFinderA=new CylinderSupportFinder();
            // var supportFinderB=new CylinderSupportFinder();
            // Vector3Wide.Broadcast(new Vector3(-0.8f,0.01f,0.71f),out var localOffsetB);
            // Matrix3x3Wide.Broadcast(Matrix3x3.CreateFromAxisAngle(new矢量3(1,0,0),0.1F),Out Var LocalOrientationB);
            // 向量3Wide.Normalize(localOffsetB,out var initialGuess);

            // GradientDescent<CylinderWide,CylinderSupportFinder,CylinderWide,CylinderSupportFinder>.Refine(a,b,localOffsetB,localOrientationB,
            // ref supportFinderA,ref supportFinderB,initialGuess,new Vector<Float>(-0.1F),new Vector<Float>(1e-4f),1500,Vector<int>.零,out var localNormal,out var Deep thBelowThreshold);

            // GJKDistanceTester<CylinderWide,CylinderSupportFinder,CylinderWide,CylinderWide,CylinderSupportFinder>gjk=默认;
            // QuaternionWide.Broadcast(Quaternion.Identity,out var localOrientationQuaternionA);
            // QuaternionWide.CreateFromRotationMatrix(localOrientationB,Out var localOrientationQuaternionB);
            // gjk.test(ref a,ref b,ref localOffsetB,ref localOrientationQuaternionA,ref localOrientationQuaternionB,out var intersected,out var Distance,out var closestA,out var gjkNormal);
            // //TimeGradientDescent(32);
            // //TimeGradientDescent(1000000);
            // }
            // {
            // 柱面宽度a=默认值;
            // a.广播(新圆筒(0.5F,1F));
            // 柱面宽度b=默认值;
            // b.广播(新气缸(0.5f,1f));
            // var supportFinderA=new CylinderSupportFinder();
            // var supportFinderB=new CylinderSupportFinder();
            // Vector3Wide.Broadcast(new Vector3(0.5F,0.5F,0.5F),out var localOffsetB);
            // Vector3Wide.Broadcast(Vector3.Normalize(new Vector3(1,0,1)),out var localCastDirection);
            // Matrix3x3Wide.Broadcast(Matrix3x3.CreateFromAxisAngle(new矢量3(1,0,0),0),Out Var LocalOrientationB);
            // mpr<CylinderWide,CylinderSupportFinder,CylinderWide,CylinderSupportFinder>.test(a,b,localOffsetB,localOrientationB,ref supportFinderA,ref supportFinderB,new Vector<浮动>(1e-5f),Vector<int>.零,out var相交,out var localNormal);
            // for(int i=0;i<5;++i)
            // {
            // mpr<CylinderWide,CylinderSupportFinder,CylinderWide,CylinderSupportFinder>.LocalSurfaceCast(a,b,localOffsetB,localOrientationB,ref supportFinderA,ref supportFinderB,localNormal,new Vector(1e-3f),Vector
            // Out Var t,Out Local Normal);
            // }
            // Vector3Wide.Normalize(局部正态,Out Var检验);

            // GJKDistanceTester<CylinderWide,CylinderSupportFinder,CylinderWide,CylinderWide,CylinderSupportFinder>gjk=默认;
            // QuaternionWide.Broadcast(Quaternion.Identity,out var localOrientationQuaternionA);
            // QuaternionWide.CreateFromRotationMatrix(localOrientationB,Out var localOrientationQuaternionB);
            // gjk.test(ref a,ref b,ref localOffsetB,ref localOrientationQuaternionA,ref localOrientationQuaternionB,out var intersected,out var Distance,out var closestA,out var gjkNormal);
            // //TimeMPRSurfaceCast(32);
            // //TimeMPRSurfaceCast(1000000);
            // }
            // {
            // 柱面宽度a=默认值;
            // a.广播(新圆筒(0.5F,1F));
            // 柱面宽度b=默认值;
            // b.广播(新气缸(0.5f,1f));
            // var supportFinderA=new CylinderSupportFinder();
            // var supportFinderB=new CylinderSupportFinder();
            // Vector3Wide.Broadcast(new Vector3(-0.335f,-0.0f,1.207f),out var localOffsetB);
            // Matrix3x3Wide.Broadcast(Matrix3x3.CreateFromAxisAngle(new矢量3(1,0,0),10.0f),外部变量定位B);
            // mpr<CylinderWide,CylinderSupportFinder,CylinderWide,CylinderSupportFinder>.test(a,b,localOffsetB,localOrientationB,ref supportFinderA,ref supportFinderB,new Vector<浮动>(1e-5f),Vector<int>.零,out var相交,out var localNormal);
            // Vector3Wide.Normalize(局部正态,Out Var检验);
            // GJKDistanceTester<CylinderWide,CylinderSupportFinder,CylinderWide,CylinderWide,CylinderSupportFinder>gjk=默认;
            // QuaternionWide.Broadcast(Quaternion.Identity,out var localOrientationQuaternionA);
            // QuaternionWide.CreateFromRotationMatrix(localOrientationB,Out var localOrientationQuaternionB);
            // gjk.test(ref a,ref b,ref localOffsetB,ref localOrientationQuaternionA,ref localOrientationQuaternionB,out var intersected,out var Distance,out var closestA,out var gjkNormal);
            // //TimeMPR(32);
            // //TimeMPR(1000000);
            // }

        }



    }
}
